#include "pid.h"


float KP = 110.0f; 
float KI =  150.0f; 
float KD =  0.0f;		


/*
 @ describetion: Moto speed PID control function
 @ param: _Moto_Str* MotoStr
 @ return: none
 @ author: Jestong Zhou
 @ date : 2019-6-14
 @ function : float HuanyuRobot_PID_Control(_Moto_Str* MotoStr)
*/
short HuanyuRobot_PID_Control(_Moto_Str* MotoStr)
{
	float Error   = 0;
	float P_Error = 0;
	float I_Error = 0;
	float D_Error = 0;
	float add     = 0;
	
	Error = MotoStr->Target_Speed  - MotoStr->Current_Speed;
	//Update the current one-two third-order error for the current proportional integral and differential calculation
	P_Error = Error;
	I_Error = Error - MotoStr->L_Error;
	D_Error = Error - 2*MotoStr->L_Error + MotoStr->LL_Error;

	//calculation current proportional integral and differential 
	add = KP * P_Error + KI * I_Error + KD * D_Error;
	MotoStr->ESC_Output_PWM += add;

	MotoStr->LL_Error = MotoStr->L_Error;
	MotoStr->L_Error = Error;

	if(MotoStr->ESC_Output_PWM > ESC_output_PWM_LIMT)	MotoStr->ESC_Output_PWM = ESC_output_PWM_LIMT;		
	else if(MotoStr->ESC_Output_PWM < 0)	MotoStr->ESC_Output_PWM = 0;
	
	return MotoStr->ESC_Output_PWM;
}


		

